/**
 * @file Tools/Math/Projection.h
 *
 * A collection of functions that are about projections and perspective and stuff.
 */

#pragma once

#include <vector>
#include "Tools/Math/Geometry.h"

struct CameraMatrix;
struct CameraInfo;
struct FieldDimensions;
struct RobotPose;

/**
 * The namespace around the functions.
 */
namespace Projection
{
  /**
   * This functions computes a polygon (of four points) described by four points.
   * This polygon describes the part of the field that can be seen in the current image.
   * All points are in field coordinates
   * @param robotPose The pose of the robot on the field
   * @param cameraMatrix The position and orientation of the robot's camera.
   * @param cameraInfo The resolution and the opening angle of the robot's camera.
   * @param fieldDimenstions The specification of the playing field
   * @param p The list of points
   */
  void computeFieldOfViewInFieldCoordinates(const RobotPose& robotPose, const CameraMatrix& cameraMatrix,
                                            const CameraInfo& cameraInfo, const FieldDimensions& fieldDimensions,
                                            std::vector<Vector2f>& p);
  
  /**
   * The function calculates the horizon.
   * @param cameraMatrix The camera matrix.
   * @param cameraInfo Object containing camera parameters.
   * @return The line of the horizon in the image.
   */
  Geometry::Line calculateHorizon(const CameraMatrix& cameraMatrix, const CameraInfo& cameraInfo);
  
  /**
   * The function approximates the shape of a ball in the camera image.
   * Note: currently, the approximation is not exact.
   * @param ballOffset The ball's position relative to the robot's body origin.
   * @param cameraMatrix The position and orientation of the robot's camera.
   * @param cameraInfo The resolution and the opening angle of the robot's camera.
   * @param ballRadius The radius of the ball in mm.
   * @param circle The approximated shape generated by the function.
   * @return If false, only the center of the circle is valid, not the radius.
   */
  bool calculateBallInImage(const Vector2f& ballOffset, const CameraMatrix& cameraMatrix,
                            const CameraInfo& cameraInfo, float ballRadius, Geometry::Circle& circle) WARN_UNUSED_RESULT;
  
  /**
   * The function determines how far an object is away depending on its real size and the size in the image.
   * @param cameraInfo Information about the camera (opening angles, resolution, etc.).
   * @param sizeInReality The real size of the object.
   * @param sizeInPixels The size in the image.
   * @return The distance between camera and object.
   */
  float getDistanceBySize(const CameraInfo& cameraInfo, float sizeInReality, float sizeInPixels);
  
  /**
   * The function determines how far an object is away depending on its real size and the size in the image
   * along with its center position, using camera intrinsic parameters.
   * @param cameraInfo Class containing the intrinsic paramaters
   * @param sizeInReality The real size of the object.
   * @param sizeInPixels The size in the image.
   * @param centerX X coordinate (in image reference) of object's baricenter.
   * @param centerY Y coordinate (in image reference) of object's baricenter.
   * @return The distance between camera and object.
   */
  float getDistanceBySize(const CameraInfo& cameraInfo, float sizeInReality, float sizeInPixels, float centerX, float centerY);
  
  /**
   * The function determines how big an object appears in the image depending on its distance and size.
   * @param cameraInfo Object containing camera parameters.
   * @param sizeInReality The real size of the object.
   * @param distance The distance to the object.
   * @return The size as it would appear in the image.
   */
  float getSizeByDistance(const CameraInfo& cameraInfo, float sizeInReality, float distance);
  
  void calculateAnglesForPoint(const Vector2f& point, const CameraMatrix& cameraMatrix, const CameraInfo& cameraInfo, Vector2f& angles);
  bool calculatePointByAngles(const Vector2f& angles, const CameraMatrix& cameraMatrix, const CameraInfo& cameraInfo, Vector2f& point) WARN_UNUSED_RESULT;
};
